#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/TransformStamped.h"

void dopose(const turtlesim::Pose::ConstPtr& pose)
{
        static  tf2_ros::TransformBroadcaster bd;
        
        geometry_msgs::TransformStamped tfs;

        tfs.header.frame_id = "world";
        tfs.child_frame_id = "turtle1";
        tfs.header.stamp = ros::Time::now();

        tfs.transform.translation.x = pose->x;
        tfs.transform.translation.y = pose->y;
        tfs.transform.translation.z = 0.0;

        tf2::Quaternion qtn;
        qtn.setRPY(0,0,pose->theta);
        tfs.transform.rotation.x = qtn.getX();
        tfs.transform.rotation.y = qtn.getY();
        tfs.transform.rotation.z = qtn.getZ();
        tfs.transform.rotation.w = qtn.getW();
        
        bd.sendTransform(tfs);
}
int main(int argc, char *argv[])
{
        ros::init(argc, argv, "turtle_tf");
        ros::NodeHandle nd;
        
        ros::Subscriber sub = nd.subscribe<turtlesim::Pose>("/turtle1/pose",1000,dopose);
        ros::spin();

        return 0;
}